/*
 * @Author: junhuizhou
 * @Date: 2020-12-30 10:55:05
 * @LastEditor: junhuizhou
 * @LastEditTime: 2021-01-05 21:05:44
 * @Description: header
 * @FilePath: \fzhtq\include\attprop.h
 */

#ifndef ATTPROP_H_
#define ATTPROP_H_

/**
 * @description: 初始化计算轨道系下attitude[7]
 * @algorithm: 本体系
 * @param att [姿态四元数+三轴角速度] 单位rad/s
 * @param angle 单位rad
 * @param anglerate 角速度 [wx,wx,wz] 单位rad/s
 * @return: 
 */
void AttInit(double att[7], double angle[3], double anglerate[3]);

/**
 * @description: 姿态运动学一步递推
 * @algorithm: RKF1 dQdt=0.5*M'[Q]*W or dQdt=0.5*M[W]*Q 两种
 * @param att [姿态四元数+三轴角速度] 单位rad/s
 * @param dh 递推步长 
 * @return: 
 */
void AttKineProp(double att[7], double dh);

/**
 * @description: 姿态动力学一步递推(内部先进行运动学递推再动力学递推)
 * @algorithm: RKF4 dwdt=I'(Tq - w x (I*w))
 * @param {type} 
 * @return:  
 */
void AttDynaProp(double att[7], double JM[3][3], double JMI[3][3], double whlAxiAngMom[3], double Tq[3], double ha);

#endif
